import java.awt.geom.Point2D;
import java.util.List;

/**
 * The main class of the system. Responsible for creating the 3 main modules of
 * the system, Chartographer/Planner and the Navigator. In addition it
 * initializes the connection to the Lokkaria server by creating an instance of
 * RobotCommunication. Once the initial setup is done the thread is responsible
 * for requesting plans from the planner and pushing them to the navigator until
 * no plan is received which means the exploration is finished.
 * 
 * @author Emanuel Yifter Lindgren (dv11elr), Anders Lumio (dv11alo)
 *
 */
public class MapMakingRobot {
    private static String host;

    public static void main(final String[] args) throws Exception {

	if (args.length != 5) {
	    System.out.println("Invalid amount of arguments.[url x1 y1 x2 y2]");
	    System.exit(1);
	}
	host = args[0];
	final RobotCommunication robotComm = new RobotCommunication(host);

	final int lowerLeftCornerOfMapX = Integer.parseInt(args[2]);
	final int lowerLeftCornerOfMapY = Integer.parseInt(args[1]);

	final int upperRightCornerOfMapX = Integer.parseInt(args[4]);
	final int upperRightCornerOfMapY = Integer.parseInt(args[3]);

	final LaserPropertiesResponse lpr = new LaserPropertiesResponse();
	final LaserEchoesResponse ler = new LaserEchoesResponse();
	final LocalizationResponse lr = new LocalizationResponse();
	robotComm.getResponse(lpr);
	robotComm.getResponse(ler);
	robotComm.getResponse(lr);

	final double[] angles = robotComm.getLaserAngles(lpr);

	final Chartographer himmChar = new HIMMChartographer(
	        lowerLeftCornerOfMapX, lowerLeftCornerOfMapY,
	        upperRightCornerOfMapX, upperRightCornerOfMapY, angles,
	        lr.getPosition()[1], lr.getPosition()[0]);

	final Planner fpp = new FrontierPathPlanner(himmChar);

	himmChar.updateMap(new MapUpdate(ler.getEchoes(), lr.getPosition(), lr
	        .getHeadingAngle()));

	final Navigator coolNavBoy = new NavigationUnit2000(robotComm,
	        himmChar, angles);

	new Thread((HIMMChartographer) himmChar).start();
	// wait for initial mapUpdate to finish.
	while (!((HIMMChartographer) himmChar).isInitialized()) {
	    Thread.sleep(100);
	}
	// This thread will pull for new plans until no plan is received, then
	// we're done.
	while (true) {
	    final List<Point2D.Double> plan = fpp.plan();

	    if (plan.isEmpty()) {
		System.out
		        .println("No more area to explore. Exiting in 5 seconds..");
		Thread.sleep(5000);
		System.exit(1);
	    }
	    coolNavBoy.navigate(plan);
	}
    }
}
